//============================================================================
// Name        : EasyParticleFilter.cpp
// Author      : 
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
using namespace std;
#include "Maze.h"
#include "Robot.h"
#include "Particle.h"
#include "WeightedDistribution.h"
#include <vector>

#define ROBOT_HAS_COMPASS true
#define PARTICLE_COUNT 2000

class BLATEST{
public :
	int i;
	int j;

	BLATEST(int i, int j):i(i),j(j){
	}

	BLATEST(const BLATEST& b){
		i = b.i;
		j = b.j;
	}

	BLATEST& operator=(const BLATEST &b){
		i = b.i;
		j = b.j;
		return *this;
	}

	~BLATEST(){
	}
};

class Blalist{
public :
	vector<BLATEST> i;
	vector<vector<int> > vec;

	Blalist():i(),vec(){

	}

	Blalist(BLATEST j):i(),vec(){
		i.push_back(j);
		vec.resize(10,vector<int>());
		for(int i = 0; i<10; i++){
			vec[i].resize(10,0);
			for(int j = 0; j < 10; j++){
				int k = (i*j);
				vec[i].push_back(k);
//				vec[i][j] = k;
			}
		}
	}

	Blalist(int d, int j):i(),vec(){
		i.push_back(BLATEST(d,j));
		vec.resize(10,vector<int>());
		for (int i = 0; i < 10; i++) {
			vec[i] = vector<int>();
			vec[i].resize(10,0);
			for (int j = 0; j < 10; j++) {
				vec[i][j] = i * j;
			}
		}
	}

	Blalist(const Blalist& b){
		i = b.i;
		vec = b.vec;
	}

	Blalist& operator=(const Blalist &b){
		i = b.i;
		return *this;
	}

	~Blalist(){
	}
};


int main() {
	int maze_data [10][10] = {
				  { 1, 1, 0, 0, 2, 0, 0, 0, 0, 1 },
	              { 1, 2, 0, 0, 1, 1, 0, 0, 0, 0 },
	              { 0, 1, 1, 0, 0, 0, 0, 1, 0, 1 },
	              { 0, 0, 0, 0, 1, 0, 0, 1, 1, 2 },
	              { 1, 1, 0, 1, 1, 2, 0, 0, 1, 0 },
	              { 1, 1, 1, 0, 1, 1, 1, 0, 2, 0 },
	              { 2, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
	              { 1, 2, 0, 1, 1, 1, 1, 0, 0, 0 },
	              { 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 },
	              { 0, 0, 1, 0, 0, 2, 1, 1, 1, 0 }};

//	cout << "!!!Hello World!!!" << endl; // prints !!!Hello World!!!
	int * mazian = maze_data[0];

//	BLATEST bla(1,1);
//	BLATEST b(bla);
//	cout << "hello!!!" <<endl;
//	Blalist lst (b);
//	cout << "hello1!!!" <<endl;
//	Blalist ls(1,1);
//
//	while(1);

	double old_heading = 0;
	cout << "Making a Maze" <<endl;
	Maze world(mazian, 10,10);
	cout << "Got a Maze" <<endl;
	Robot robbie(&world);
	cout << "Got a Robot" <<endl;
//	cout << "Robbies Position: ["<<robbie.getPosition().x << ";"<<robbie.getPosition().y<<"]"<<endl;
//	world.draw();
//	while (1) {
//		world.redraw();
//		world.drawRobot(robbie.getPosition());
//	};
	srand(time(NULL));
	vector<Particle> particles;
	vector<Particle> * parts = &particles;
	Particle::create_random(parts,PARTICLE_COUNT,&world);
	cout << "Got a Particles" <<endl;
	vector<Particle> new_particles;
	new_particles.reserve(PARTICLE_COUNT);
	parts = &new_particles;
	cout << "drawing maze"<<endl;
	world.draw();
	while(true){
//		cout << "jojo"<<endl;
//		world.redraw();
//		cout << "jojo2"<<endl;
//		world.drawRobot(robbie.getPosition());
//		cout << "Robbies Position: ["<<robbie.getPosition().x << ";"<<robbie.getPosition().y<<"]"<<endl;
//		cout << "jojo3"<<endl;
		double r_d = robbie.read_sensor();
//		cout << "robbie got sensordata: "<< r_d<<endl;
		for(int i = 0; i < PARTICLE_COUNT;i++){
			if(world.is_free(particles[i].p)){
				double p_d = particles[i].read_sensor(&world);
				particles[i].w = Particle::w_gauss(r_d,p_d);
			}else{
				particles[i].w = 0;
			}
		}
//		cout << "particles read"<< endl;
		// displays ...
		world.redraw();
		world.drawRobot(robbie.getPosition());
		for(unsigned int i = 0; i < particles.size(); i++){
			world.drawParticles(particles[i].p);
		}
		world.repaint();
		//world.show_particles(particles);
		//world.show_mean(m_x,m_y, m_confident);
		//world.show_robot(robbie);

		//normalize
		double nu = 0;
		for( unsigned int i = 0; i < particles.size(); i++){
			nu+= particles[i].w;
		}
		for (unsigned int i = 0; i < particles.size(); i++) {
			particles[i].w /= nu;
		}
//		cout << "Nu:" << nu << endl;
//		cout << "particles got normalized"<< endl;
		//weighted distri
		WeightedDistribution dist(particles);
		//pick
//		cout << "got weighted Distribution"<<endl;
		Particle p;
//		vector<Particle> *
//		cout << dist.states.size() << endl;
		for( unsigned int i = 0; i < PARTICLE_COUNT; i++){
			p = dist.pick();
			if( p.x == -1 && p.y == -1){
//				cout << " need to pick a random "<< i << endl;
				Particle::create_random(parts/*&new_particles*/,1,&world);
//				cout << "picked a random, size: " << new_particles.size() << endl;
//				cout << dist.states.size() << endl;
			}else{
				if(dist.states.size() == 0){
					cout << "how comes I am here?" << endl;
				}
				new_particles.push_back(Particle(p.x,p.y,(ROBOT_HAS_COMPASS?robbie.getHeading():p.heading),true));
			}
		}
//		cout << "finished picking particles " << endl;
		particles.clear();
//		particles.vector(new_particles.get_allocator());
//		particles.vector(new_particles);

		particles.insert(particles.begin(),new_particles.begin(), new_particles.end());
//		cout << "inserted all new particles: " << new_particles.size() << " vs. " << particles.size() <<endl;
		new_particles.clear();
		//move things
		old_heading = robbie.getHeading();
//		cout << "now moving robbie!"<<endl;
		robbie.move();
//		cout << "robbie has been moved" << endl;
		double d_h = robbie.getHeading() - old_heading;
//		cout << "This is Robbies Position: ["<<robbie.getPosition().x << ";"<<robbie.getPosition().y<<"]"<<endl;
		//move particles
//		cout << "particles have to move too" << endl;
		for(unsigned int i = 0; i < particles.size(); i++){
			particles[i].heading += d_h;
			particles[i].advance_by(robbie.speed,&world);
		}
//		cout << "all particles moved!" <<endl;
	}
	world.releaseImage();
	return 0;
}
